
#include "AManSta.h"
#include "./Global_Func/MEM_Operation/MEM_Operation.h"
AManSta_T* AManSta_t;

Track_Sta TS;
Force_MRE MRE;
Track_Sta SP_TR;
Track_Sta PV_TR;
Force_MRA MRA;

//////************************************
//// Method:    	CALCU_SAMA					////< SAMA运算函数
//// Programmer:  duan
//// Time:		2019/06/12
//// Returns:   	bool						////< 成功：true；失败：false
//// Parameter: 	Model_Base_t * arg			////< 模块参数结构体指针
//// Parameter: 	float dt					////< 运算周期
//// Parameter: 	bool * DI					////< 数字量输入值数组指针
//// Parameter: 	bool * DO					////< 数字量输出值数组指针
//// Parameter: 	bool * AI					////< 模拟量输入值数组指针
//// Parameter: 	bool * AO					////< 模拟量输出值数组指针
//////************************************
bool CALCU_SAMA(AManSta)(Model_Base_t* arg, float dt, bool* DI, bool* DO, float* AI, float* AO)
{
	/*真的恶心***************变量定义区开始**************/

	float Midvar = 0;		////< 用于翻转
	/*真的恶心<<<<<定义输入变量>>>>>*/
	float X_Value;			////< 输入值
	float FF_Value;			////< 前馈值
	float TR_Value;			////< 跟踪值
	float YP_Value;			////< 位置反馈值
	float PV_Value;			////< 过程值
	float SP_InValue;		////< 设定值

	////< Track_Sta TS;		////< 跟踪 0：不跟踪 1：跟踪
	bool BI;				////< 闭锁增 1：生效
	bool BD;				////< 闭锁减 1：生效
	////< Force_MRE MRE;		////< 强制手动 1：生效
	////< Force_MRA MRA;		////< 强制自动 1：生效
	////< Track_Sta PV_TR;	////< (AO)SP跟踪(AI)PV 1：生效
	////< Track_Sta SP_TR;	////< (AO)SP跟踪(AI)SP 1：生效

	/*真的恶心<<<<<定义输出变量>>>>>*/
	float Y_Value = 0;
	float SP_OutValue = 0;

	AM_Sta MA_StaOut;		////< 手操器状态 0：手动  1：自动
	bool INC_StaOut;		////< 输出增信号
	bool DEC_StaOut;		////< 输出减信号

	/*真的恶心<<<<<定义参数变量>>>>>*/
	float K;					////< 输出增益
	float Bias;					////< 输出偏置
	float Y_High;				////< Y输出上限
	float Y_Low;				////< Y输出下限
	float SP_High;				////< SP输出上限
	float SP_Low;				////< SP输出下限
	float Trate;				////< 变化率
	float DeadBand;				////< 死区
	float OnTime;				////< 高电平宽度
	float OffTime;				////< 低电平宽度
	bool TurnOver;				////< 输出反向or输出正向
	AM_Sta FP;					////< 初始化方式（0：手动 1：自动）
	AM_Sta ManF;				////< 手动禁止 (0:允许手动 1：禁止手动)
	Work_Mode Mode;				////< 工作方式 (0:Normal;1:Electric)
	Electric_Mode EMode;		////< Electric输出方式(0:长信号;1:脉冲)

	/*真的恶心<<<<<定义中间变量>>>>>*/
	float* ManOut;
	float* SP_ManOut;
	AM_Sta* MA_Sta;
	int* Y_Press;
	float* Y_PressTime;
	float* Y_Rate;
	int* SP_Press;
	float* SP_PressTime;
	float* SP_Rate;

	float* INCCounte;
	float* DECCounte;
	bool* Last_MRA;
	float* Last_Y_Value;

	int* Init_Count;

	/*真的恶心***************变量定义区结束**************/

	NAME(AManSta)* Obj = (NAME(AManSta)*)arg;

	/*真的恶心***************变量赋值**************/
	/*真的恶心<<<<<输入变量赋值>>>>>*/
	X_Value = AI[0];
	FF_Value = AI[1];
	TR_Value = AI[2];
	YP_Value = AI[3];
	PV_Value = AI[4];
	SP_InValue = AI[5];

	TS = (Track_Sta)DI[0];
	BI = DI[1];
	BD = DI[2];
	MRE = (Force_MRE)DI[3];
	MRA = (Force_MRA)DI[4];
	PV_TR = (Track_Sta)DI[5];
	SP_TR = (Track_Sta)DI[6];

	/*真的恶心<<<<<参数变量赋值>>>>>*/
	K = Obj->Par.K;					////< 输出增益
	Bias = Obj->Par.Bias;			////< 输出偏置
	Y_High = Obj->Par.Y_High;		////< Y输出上限
	Y_Low = Obj->Par.Y_Low;			////< Y输出下限
	SP_High = Obj->Par.SP_High;		////< SP输出上限
	SP_Low = Obj->Par.SP_Low;		////< SP输出下限
	Trate = Obj->Par.Trate;			////< 变化率
	DeadBand = Obj->Par.DeadBand;	////< 死区
	OnTime = Obj->Par.OnTime;		////< 高电平宽度
	OffTime = Obj->Par.OffTime;		////< 低电平宽度
	TurnOver = Obj->Par.TurnOver;	////< 输出反向or输出正向
	FP = Obj->Par.FP;				////< 初始化方式（0：手动 1：自动）
	ManF = Obj->Par.ManF;			////< 手动禁止 (0:允许手动 1：禁止手动)
	//ManF = AUTO;                   ////< bai20200910 ceshi
	Mode = Obj->Par.Mode;			////< 工作方式 (0:Normal;1:Electric)
	EMode = Obj->Par.EMode;			////< Electric输出方式(0:长信号;1:脉冲)

	/*真的恶心<<<<<中间变量赋值>>>>>*/
	ManOut = &(Obj->Sta.ManOut);
	SP_ManOut = &(Obj->Sta.SP_ManOut);
	MA_Sta = &(Obj->Sta.MA_Sta);
	Y_Press = &(Obj->Sta.Y_Press);
	Y_PressTime = &(Obj->Sta.Y_PressTime);
	Y_Rate = &(Obj->Sta.Y_Rate);
	SP_Press = &(Obj->Sta.SP_Press);
	SP_PressTime = &(Obj->Sta.SP_PressTime);
	SP_Rate = &(Obj->Sta.SP_Rate);

	INCCounte = &(Obj->Sta.INCCounte);
	DECCounte = &(Obj->Sta.DECCounte);
	Last_MRA = &(Obj->Sta.Last_MRA);
	Last_Y_Value = &(Obj->Sta.Last_Y_Value);
	Init_Count = &(Obj->Sta.Init_Count);

	/*真的恶心*************变量赋值结束************/

	/*真的恶心*******************************************/
	/*真的恶心*************算法功能实现开始**************/
	/*真的恶心*******************************************/
	if (Init_Count[0] == 0)							////< 初始化
	{
		if (ManF == AUTO && FP == MAN)				////< 允许手动&&初始化方式为手动
			MA_Sta[0] = MAN;						////< 手动

		if (FP == AUTO)								////< 初始化方式为自动
			MA_Sta[0] = AUTO;						////< 自动

		Init_Count[0] += 1;
	}

	////if (Last_MRA[0] == NO_Force_AUTO && MRA == Yes_Force_AUTO)		
	////	MA_Sta[0] = AUTO;
	/*真的恶心Last_MRA[0] = MRA;*/
	if (MRE == Yes_Force_MAN && ManF == MAN_ALLOW && MRA == NO_Force_AUTO)						////< 手动请求来，切手动
		MA_Sta[0] = MAN;
	if (MRA == Yes_Force_AUTO && MRE == NO_Force_MAN)									////< 自动请求信号来，切自动 	
		MA_Sta[0] = AUTO;






	if (SP_TR == Yes_TR)							////< SP跟踪
	{
		SP_OutValue = SP_InValue;
		SP_ManOut[0] = SP_OutValue;
	}
	else
	{
		if (PV_TR == Yes_TR)						////< PV跟踪
		{
			SP_OutValue = PV_Value;
			SP_ManOut[0] = SP_OutValue;
		}
		else
			SP_OutValue = SP_ManOut[0];

		////else										////< MSP命令
		////{
		////	if (MRE == NO_Force_MAN && MRA == NO_Force_AUTO)
		////	{
		////		if (SP_Press[0] == 1)
		////			SP_PressTime[0] += dt;
		////		else if (SP_Press[0] == 0)
		////			SP_PressTime[0] = 0;

		////		if (SP_PressTime[0] > 2)
		////			SP_OutValue += SP_Rate[0];
		////	}
		////	
		////}
	}


	if (MA_Sta[0] == AUTO && TS == NO_TR)			////< 自动模式
	{
		MA_StaOut = AUTO;
		Y_Value = (K*X_Value + Bias) + FF_Value;

		ManOut[0] = Y_Value;

		Midvar = Y_Value;							////< 翻转-输出正向or输出反向
		if (TurnOver == false)
			Y_Value = Midvar;
		if (TurnOver == true)
			Y_Value = Y_High - Midvar + Y_Low;

		if (BI == true)								////< 闭锁增
		{
			if (Y_Value > Last_Y_Value[0])
				Y_Value = Last_Y_Value[0];
		}

		if (BD == true)								////< 闭锁减
		{
			if (Y_Value < Last_Y_Value[0])
				Y_Value = Last_Y_Value[0];
		}
	}

	if (MA_Sta[0] == MAN && TS == NO_TR)			////< 手动模式
	{
		MA_StaOut = MAN;
		Y_Value = ManOut[0];

		////if (Y_Press[0] == 1)						////< MAO命令
		////	Y_PressTime[0] += dt;
		////else if (Y_Press[0] == 0)
		////	Y_PressTime[0] = 0;

		////if (Y_PressTime[0] <= 2)
		////	Y_Value = ManOut[0];
		////else
		////{
		////	Y_Value += Y_Rate[0] * dt;
		////	ManOut[0] += Y_Rate[0] * dt;
		////}

		Midvar = Y_Value;							////< 翻转-输出正向or输出反向
		if (TurnOver == false)
			Y_Value = Midvar;
		if (TurnOver == true)
			Y_Value = Y_High - Midvar + Y_Low;

		if (BI == true)								////< 闭锁增
		{
			if (Y_Value > Last_Y_Value[0])
			{
				Y_Value = Last_Y_Value[0];
				ManOut[0] = Y_Value;
			}

		}

		if (BD == true)								////< 闭锁减
		{
			if (Y_Value < Last_Y_Value[0])
			{
				Y_Value = Last_Y_Value[0];
				ManOut[0] = Y_Value;
			}

		}

	}

	if (TS == Yes_TR)								////< Y输出跟踪
	{
		Y_Value = TR_Value;
		ManOut[0] = Y_Value;
		/*真的恶心MA_Sta[0] = MAN;*/						////< duan0721 修改，特此记录
		MA_StaOut = MAN;
	}

	if (SP_OutValue > SP_High)						////< 设定值SP输出上下限设置
		SP_OutValue = SP_High;
	if (SP_OutValue < SP_Low)
		SP_OutValue = SP_Low;

	if (Trate != 0)									////< Y变化率限制		上位输入“切换变化率”时只可以为正数
	{
		if ((Y_Value - Last_Y_Value[0]) / dt > Trate)
			Y_Value = Last_Y_Value[0] + Trate * dt;

		if ((Y_Value - Last_Y_Value[0]) / dt < (-Trate))
			Y_Value = Last_Y_Value[0] - Trate * dt;
	}

	if (Y_Value > Y_High)							////< Y输出上限 输出下限
	{
		Y_Value = Y_High;
		ManOut[0] = Y_High;
	}
	if (Y_Value < Y_Low)
	{
		Y_Value = Y_Low;
		ManOut[0] = Y_Low;
	}


	if (Y_Value > (YP_Value + DeadBand) && Mode == Electric)		////<  软伺放（ELECTRIC）工作方式
	{
		if (EMode == Pulse)
		{
			DECCounte[0] = 0;
			if (INCCounte[0] < OnTime)
				INC_StaOut = true;
			if (INCCounte[0] > OnTime)
				INC_StaOut = false;
			if (INCCounte[0] > OnTime + OffTime)
				INCCounte[0] = 0;

			INCCounte[0] += dt;

			DEC_StaOut = false;
		}
		else
		{
			INC_StaOut = true;
			DEC_StaOut = false;
		}
	}
	else if (Y_Value < (YP_Value - DeadBand) && Mode == Electric)
	{
		if (EMode == Pulse)
		{
			INCCounte[0] = false;
			if (DECCounte[0] < OnTime)
				DEC_StaOut = true;
			if (DECCounte[0] > OnTime)
				DEC_StaOut = false;
			if (DECCounte[0] > OnTime + OffTime)
				DECCounte[0] = 0;

			DECCounte[0] += dt;

			INC_StaOut = false;
		}
		else
		{
			INC_StaOut = false;
			DEC_StaOut = true;
		}
	}
	else
	{
		INC_StaOut = false;
		DEC_StaOut = false;
	}

	if (Mode == Normal)												////<  正常（Normal）工作方式
	{
		INC_StaOut = false;
		DEC_StaOut = false;
	}

	Last_Y_Value[0] = Y_Value;



	/*真的恶心*************算法功能实现结束**************/
	/*真的恶心*******************************************/
	/*真的恶心***************运算结果输出**************/
	AO[0] = Y_Value;
	AO[1] = SP_OutValue;

	DO[0] = MA_StaOut;
	DO[1] = INC_StaOut;
	DO[2] = DEC_StaOut;

	/*真的恶心*************运算结果输出完毕************/
	return true;
}



//////************************************
//// Method:    	BCPARA						////< 广播SAMA参数变量函数（无参数需要广播的模块直接将BCPara函数设置为NULL即可）
//// Programmer:  duan
//// Time:		2019/06/12
//// Returns:   	int							////< 成功：返回打包字节个数；失败：返回0
//// Parameter: 	Model_Base_t * arg			////< 模块参数结构体指针
//// Parameter: 	void ** Data				////< 广播SAMA参数的数据包的指针
//////************************************
int BC_PARA(AManSta)(Model_Base_t* arg, void** Data)
{
	NAME(AManSta)* Obj = (NAME(AManSta)*)arg;
	BC_PARA_PUBLIC(Obj);
	return 0;
}



//////************************************
//// Method:    	BAK_STA						////< 备份SAMA运算中间参数
//// Programmer:  duan
//// Time:		2019/06/12
//// Returns:   	int							////< 成功：返回打包字节个数；失败：返回0
//// Parameter: 	Model_Base_t * arg			////< 指定模块类型链表的指针
//// Parameter: 	void ** Data				////< 存放数据包当前指针位置的地址
//////************************************
int BAK_STA(AManSta)(Model_Base_t* arg, void** Data)
{
	NAME(AManSta)* Obj = (NAME(AManSta)*)arg;
	BAK_STA_PRIVATE(Obj);
	return 0;
}



//////************************************
//// Method:    	ANA_STA							////< 解析备份的SAMA运算中间参数
//// Programmer:  duan
//// Time:		2019/06/12
//// Returns:   	int								////< 成功：返回解析字节个数；失败：返回0
//// Parameter: 	Model_Base_t * arg				////< 模块参数结构体指针
//// Parameter: 	void ** Data					////< 存放数据包当前指针位置的地址
//////************************************
int ANA_STA(AManSta)(Model_Base_t* arg, void** Data)
{
	NAME(AManSta)* Obj = (NAME(AManSta)*)arg;
	ANA_STA_PRIVATE(Obj);
	return 0;
}



//////************************************
//// Method:    	CHANGE_PAR					////< 在线修改模块参数函数
//// Programmer:  duan
//// Time:		2019/06/12
//// Returns:   	int							////< 读取字节长度计数值
//// Parameter: 	Model_Base_t* arg			////< 指定模块类型链表的指针
//// Parameter: 	void** Data					////< 存放数据包当前指针位置的地址
//////************************************
int CHANGE_PAR(AManSta)(Model_Base_t* arg, void** Data)
{
	int _iLen = 0;
	MEMREAD(((NAME(AManSta)*)arg)->Par, Data[0]);
	return _iLen;
}



//////************************************
//// Method:    	INIT_STAPAR					////< 初始化模块运算中间参数
//// Programmer:  duan
//// Time:		2019/06/12
//// Parameter: 	Model_Base_t* arg			////< 模块参数结构体指针
//////************************************
void INIT_STAPAR(AManSta)(Model_Base_t* arg)
{
	NAME(AManSta)* name = (NAME(AManSta)*)arg;
	///****************初始化中间变量结构体**************/
	memset(&(name->Sta), 0, sizeof(name->Sta));
	///**************初始化中间变量结构体结束************/

	///****************对中间变量结构体的相关参数进行赋值**************/

	///**************对中间变量结构体的相关参数进行赋值结束************/
}



//////************************************
//// Method:    	CMD_DEAL					////< 与模块相关的命令字处理函数
//// Programmer:  duan
//// Time:		2019/06/12
//// Returns:   	void					
//// Parameter: 	Model_Base_t* arg			////< 指定模块类型链表的指针
//// Parameter: 	void* Data					////< 信息头指针
//// Parameter: 	char* CMD					////< 命令字
//////************************************
void CMD_DEAL(AManSta)(Model_Base_t* arg, void* Data, const char* CMD)
{
#define	MEMREAD1(var)	MEMREAD(var, Data)

	int _iLen = 0;
	float tmp_float;
	int tmp_Init;

	NAME(AManSta)* name = (NAME(AManSta)*)arg;

	if (!strcmp(CMD, "FAO"))								////< Y直接输出(手动模式下工作)
	{
		if (name->Sta.MA_Sta == MAN && TS == NO_TR) //< bai 20200916 修改
			MEMREAD1(name->Sta.ManOut);
	}
	else if (!strcmp(CMD, "MSP"))							////< SP手动增、手动减
	{
		MEMREAD1(tmp_float);								////< 数值 
		MEMREAD1(name->Sta.SP_Rate);						////< 变化率  上位中下发的变化率始终为零

		if (SP_TR == NO_TR && PV_TR == NO_TR)
		{
			float LimitJudgeTempVariable = name->Sta.SP_ManOut + tmp_float;//< ShiKe20200716注释：这一段都是新加的
			if (LimitJudgeTempVariable > name->Par.SP_High)				//< ShiKe20200716注释：加这一段的目的是，当设定值超限时，上位看到的输出是限制到了上下限，但是下位的存储量还在增加或减少
				name->Sta.SP_ManOut = name->Par.SP_High;				//< ShiKe20200716注释：举个例子，如果没有这一段，SP上限是10，当从9开始增加1点了两次，上位显示的是10，下位记录的是11，导致上位点击减少1一次，上位显示的还是10，再点1次才是9
			else if (LimitJudgeTempVariable < name->Par.SP_Low)			//< ShiKe20200716注释：所以加了这一段，就可以保证达到限幅时，反方向操作可以让上位输出马上变化，不会被误认为模块没执行操作。
				name->Sta.SP_ManOut = name->Par.SP_Low;
			else
				name->Sta.SP_ManOut += tmp_float;						//< ShiKe20200716注释：原先只有这一句而没有上面这一段
		}
	}
	else if (!strcmp(CMD, "MAO"))							////< Y手动减、手动增(手动模式下工作)
	{
		MEMREAD1(tmp_float);								////< 数值
		MEMREAD1(name->Sta.Y_Rate);							////< 变化率  上位中下发的变化率始终为零

		if (name->Sta.MA_Sta == MAN && name->Par.ManF == MAN_ALLOW && TS == NO_TR)
			name->Sta.ManOut += tmp_float;

		///////*name->Sta.Y_Press = 1;
		//name->Sta.Y_PressTime = 0;*/
	}
	else if (!strcmp(CMD, "HAO"))							////< 与MAO命令成对出现
	{
		///////*name->Sta.SP_Press = 0;
		//name->Sta.Y_Press = 0;*/
	}
	else if (!strcmp(CMD, "FSP"))							////< SP直接输出
	{
		MEMREAD1(tmp_float);
		/////*name->Sta.SP_Press = 0;*/
		if (tmp_float > name->Par.SP_High)
			tmp_float = name->Par.SP_High;
		if (tmp_float < name->Par.SP_Low)
			tmp_float = name->Par.SP_Low;

		if (name->Sta.MA_Sta == MAN && name->Par.ManF == MAN_ALLOW && TS == NO_TR)
			name->Sta.SP_ManOut = tmp_float;
	}

	else if (!strcmp(CMD, "MDS"))							////< 手动、自动切换
	{
		MEMREAD1(tmp_Init);
		if (TS == NO_TR)
		{
			if (tmp_Init == 0)								////< 手动
			{
				if (name->Par.ManF == MAN_ALLOW && MRA == NO_Force_AUTO)
					name->Sta.MA_Sta = MAN;
			}

			if (tmp_Init == 1)								////< 自动
			{
				//if (MRE == NO_Force_MAN)
					name->Sta.MA_Sta = AUTO;
			}
		}

	}

#undef MEMREAD1
}
